 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Introduction to the KDL Manager
Description: How to initialize the KDL manager and load a kinematic chain's information from the ROS parameter server.Keywords: KDL, KDLManager, generic_control_toolbox, control
Tutorial Level: BEGINNER
Next Tutorial: Basic control algorithm
Contents
Introduction
The KDLManager class encapsulates useful methods for handling KDL when implementing a robot controller in ROS. In this tutorial, we will guide you through the process of building a ROS package which uses the KDLManager to load and manage a kinematic chain.
Setting up the workspace
We will need the generic_control_toolbox and a robot description to be available for this tutorial. We will use the Baxter common package to provide a robot description.
$ mkdir -p ~/catkin_ws/src && cd ~/catkin_ws/src $ catkin_init_workspace $ git clone https://github.com/diogoalmeida/generic_control_toolbox.git $ git clone https://github.com/RethinkRobotics/baxter_common.git
And make a new package
$ mkdir -p kdl_manager_tutorial/src
The code
We will create a ROS node which initializes a KDL Manager instance and, from available JointState information, compute the end-effector and a configured gripping point poses of Baxter's left arm.
First, move to the source folder
$ cd kdl_manager_tutorial/src
and create a CPP file named 'load_chain.cpp' with the following code
1 #include <ros/ros.h>
2 #include <generic_control_toolbox/kdl_manager.hpp>
3
4
5 int main (int argc, char ** argv)
6 {
7 ros::init(argc, argv, "load_chain");
8
9 // Initialize a KDL manager instance with the 'torso' link configured as its
10 // base frame.
11 generic_control_toolbox::KDLManager manager("torso");
12
13 // Load a kinematic chain which goes from the base frame ('torso') to a
14 // user-configured end-effector ('left_hand')
15 manager.initializeArm("left_hand");
16 KDL::Frame pose;
17 sensor_msgs::JointState state;
18
19 // The KDL manager parses the robot state information from a JointState message,
20 // which robots using ROS will publish on the network.
21 state = *ros::topic::waitForMessage<sensor_msgs::JointState>("/joint_states");
22
23 // Get the end-effector pose as a KDL::Frame.
24 manager.getEefPose("left_hand", state, pose);
25 ROS_INFO_STREAM("The left hand end-effector position is " << pose.p.x() << ", " << pose.p.y() << ", " << pose.p.z());
26
27 // Configure a gripping point on the robot
28 manager.setGrippingPoint("left_hand", "left_gripper");
29
30 // Get the pose of the gripping point
31 manager.getGrippingPoint("left_hand", state, pose);
32 ROS_INFO_STREAM("The left hand gripping point position is " << pose.p.x() << ", " << pose.p.y() << ", " << pose.p.z());
33 }
The Code Explained
We will break down the Manager's calls
11 generic_control_toolbox::KDLManager manager("torso");
On construction, a KDL Manager receives the robot's base frame name, from which the kinematic chains will spawn.
The KDL manager maintains a set of kinematic chains. Here, we load the chain that goes from our configured base frame ('torso') to the declared end-effector ('left_hand'). We additionally initialize the data structures needed to get kinematic information from a chain, a KDL Frame to store the pose information and a sensor_msgs JointState message to hold the robots state information.
We obtain the current robot state information from the joint_states topic and used the getEefPose method to compute the forward kinematics of the loaded joint chain.
Finally, we set the 'left_hand' frame to be a configured gripping point and compute its pose. Any TF frame can be set as a gripping point. Note: The manager assumes that the relationship between the end-effector frame and the gripping point frame is static.
Building the node
In the package root, add the following CMakeLists.txt file:
1 cmake_minimum_required(VERSION 2.8.3)
2 project(kdl_manager_tutorial)
3
4 find_package(
5 catkin REQUIRED COMPONENTS
6 roscpp
7 generic_control_toolbox
8 )
9
10 add_definitions(-std=c++11)
11 link_directories(${catkin_LIBRARY_DIRS})
12
13 catkin_package(
14 CATKIN_DEPENDS roscpp generic_control_toolbox
15 )
16
17 include_directories(
18 ${catkin_INCLUDE_DIRS}
19 )
20
21 add_executable(load_chain src/load_chain.cpp)
22 target_link_libraries(load_chain ${catkin_LIBRARIES})
23 add_dependencies(load_chain ${catkin_EXPORTED_TARGETS})
And in your package.xml file do not forget to include
... <depend>roscpp</depend> <depend>generic_control_toolbox</depend> ...
Running your code
To use the KDL manager we need a joint_states message to be available through a ROS topic. This is the case when using a real robot or proper simulation. For the purposes of this tutorial, we will use Baxter's description and the 'joint_state_publisher' package to generate a joint_states message. Make a 'launch' directory
$ mkdir launch
And add a 'tutorial.launch' launch file
<launch>
<param name="robot_description" command="$(find xacro)/xacro.py --inorder $(find baxter_description)/urdf/baxter.urdf.xacro gazebo:=false"/>
<node pkg="kdl_manager_tutorial" type="load_chain" name="load_chain" output="screen"/>
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
<param name="publish_default_velocities" type="bool" value="true" />
</node>
</launch>and, after compiling and sourcing your workspace, the code can now be tested with
$ roslaunch kdl_manager_tutorial tutorial.launch
